/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Vector2.h"

namespace Robotic { namespace Math{
	Vector2::Vector2(void)
	{
		_datas[0] = _datas[1] = 0.0;
	}

	Vector2::Vector2(const double x,const double y)
	{
		_datas[0] = x;
		_datas[1] = y;
	}

	Vector2::Vector2(const Vector2 & arg)
	{
		_datas[0] = arg._datas[0];
		_datas[1] = arg._datas[1];
	}

	Vector2::~Vector2(void)
	{
	}

	double& Vector2::operator [] (const int index)
	{
		if((index < 0) || (index > 1))
			throw(new std::exception("Argument index must be between 0 and 1."));

		return _datas[index];
	}

	double Vector2::operator [] (const int index) const
	{
		if((index < 0) || (index > 1))
			throw(new std::exception("Argument index must be between 0 and 1."));

		return _datas[index];
	}

	Vector2& Vector2::operator = (const Vector2 & arg)
	{
		_datas[0] = arg._datas[0];
		_datas[1] = arg._datas[1];
		return *this;
	}

	Vector2& Vector2::operator +=(const Vector2 & arg)
	{
		_datas[0] += arg._datas[0];
		_datas[1] += arg._datas[1];
		return *this;
	}

	Vector2& Vector2::operator -= (const Vector2 & arg)
	{
		_datas[0] -= arg._datas[0];
		_datas[1] -= arg._datas[1];
		return *this;
	}

	bool operator == ( const Vector2& lhs, const Vector2& rhs )
	{
		return Equal(lhs,rhs);
	}

	bool operator != ( const Vector2& lhs, const Vector2& rhs ) 
	{
		return !Equal(lhs,rhs);
	}

	Vector2 operator + ( const Vector2& lhs, const Vector2& rhs )
	{
		return Vector2(lhs._datas[0] + rhs._datas[0],lhs._datas[1] + rhs._datas[1]);
	}

	Vector2 operator - (const Vector2& lhs, const Vector2 & rhs)
	{
		return Vector2(lhs._datas[0] - rhs._datas[0], lhs._datas[1] - rhs._datas[1]);
	}

	Vector2 operator - (const Vector2& arg)
	{
		return Vector2(- arg._datas[0], - arg._datas[1]);
	}

	Vector2 operator * (const Vector2& lhs, const double rhs) 
	{
		return Vector2(lhs._datas[0] * rhs,lhs._datas[1] * rhs);
	}

	Vector2 operator * (const double lhs,const Vector2& rhs)
	{
		return Vector2(lhs * rhs._datas[0], lhs * rhs._datas[1]);
	}

	double operator * (const Vector2 lhs,const Vector2& rhs)
	{
		return lhs._datas[0] * rhs._datas[0] + lhs._datas[1] * rhs._datas[1];
	}

	Vector2 operator / (const Vector2& lhs, const double rhs)
	{
		return Vector2(lhs._datas[0] / rhs, lhs._datas[1] / rhs);
	}

	void Vector2::Normalize() 
	{
		double v = this->Norm();
		if (v <= 0) 
		{
			_datas[0] = 1;
			_datas[1] = 0;
		} 
		else
		{
			_datas[0] = _datas[0] / v;
			_datas[1] = _datas[1] / v;
		}
	}

	double Vector2::Norm() const
	{
		return sqrt(sqr(_datas[0]) + sqr(_datas[1]));
	}

	void Vector2::ReverseSign()
	{
		_datas[0] = -_datas[0];
		_datas[1] = -_datas[1];
	}

	void Vector2::SetZero()
	{
		_datas[0] = 0;
		_datas[1] = 0;
	}

	Vector2 Vector2::Zero()
	{
		return Vector2(0,0);
	}

	//void Vector2::Set3DXY(const Vector3& v)
	//	// projects v in its XY plane, and sets *this to these values
	//{
	//	_datas[0] = v[0];
	//	_datas[1] = v[1];
	//}

	//void Vector2::Set3DYZ(const Vector3& v)
	//	// projects v in its XY plane, and sets *this to these values
	//{
	//	_datas[0] = v[1];
	//	_datas[1] = v[2];
	//}
	//void Vector2::Set3DZX(const Vector3& v)
	//	// projects v in its XY plane, and and sets *this to these values
	//{
	//	_datas[0] = v[2];
	//	_datas[1] = v[0];
	//}

	//// projects v in the XY plane of frame, and sets *this to these values
	//// expressed wrt some frame.
	//void Vector2::Set3DPlane(const Frame3& frame,const Vector3& v) 
	//{
	//	Vector3 tmp = frame.Inverse(v);
	//	_datas[0] = tmp[0];
	//	_datas[1] = tmp[1];
	//}

	bool Equal(const Vector2& a,const Vector2& b,const double eps) 
	{
		return ( Equal(a._datas[0],b._datas[0],eps) && Equal(a._datas[1],b._datas[1],eps) );
	}

	//Vector2 AddDelta(const Vector2& a,const Vector2& da,double dt)
	//{
	//	return a + da * dt;
	//}

	//Vector2 Diff(const Vector2& a,const Vector2& b,double dt)
	//{
	//	return ( b-a ) / dt;
	//}

	void Random(Vector2& a) 
	{
		Random(a[0]);
		Random(a[1]);
	}
	void PosRandom(Vector2& a) 
	{
		PosRandom(a[0]);
		PosRandom(a[1]);
	}
}}
